import os
import open3d as o3d
import glob


# ------- 硬编码参数 ----------
input_pcd_path = "../maps/partmap2.pcd"
output_pcd_path = "../maps/partmap2_downsampled.pcd"


def downsample_pcd(input_path, output_path, voxel_size):
    """
    对输入路径下的 PCD 文件进行降采样并保存到输出路径
    
    参数:
    input_path (str): 输入路径（可以是单个文件或目录）
    output_path (str): 输出路径（可以是文件或目录）
    voxel_size (float): 体素降采样的尺寸
    """
    # 创建输出目录（如果不存在）
    os.makedirs(os.path.dirname(output_path), exist_ok=True)
    
    # 处理单个文件
    if input_path.lower().endswith('.pcd'):
        _process_single_file(input_path, output_path, voxel_size)
    
    # 处理整个目录
    elif os.path.isdir(input_path):
        # 获取目录中所有PCD文件
        pcd_files = glob.glob(os.path.join(input_path, '*.pcd'))
        
        # 如果输出路径是目录，保持原始文件名
        if os.path.isdir(output_path) or not output_path.endswith('.pcd'):
            for pcd_file in pcd_files:
                filename = os.path.basename(pcd_file)
                out_file = os.path.join(output_path, filename)
                _process_single_file(pcd_file, out_file, voxel_size)
        # 如果输出路径是文件，则只处理第一个文件（警告）
        else:
            print(f"警告: 输入是目录但输出是单个文件，只处理第一个文件")
            _process_single_file(pcd_files[0], output_path, voxel_size)
    
    else:
        raise ValueError("无效的输入路径，必须是PCD文件或目录")

def _process_single_file(input_file, output_file, voxel_size):
    """处理单个PCD文件"""
    # 读取点云
    pcd = o3d.io.read_point_cloud(input_file)
    
    # 检查点云是否为空
    if not pcd.has_points():
        raise ValueError(f"无法读取点云文件或文件为空: {input_file}")
    
    # 执行体素降采样
    downsampled = pcd.voxel_down_sample(voxel_size)
    
    # 保存结果
    o3d.io.write_point_cloud(output_file, downsampled)
    print(f"降采样完成: {input_file} -> {output_file} (原始点数: {len(pcd.points)}, 采样后: {len(downsampled.points)})")


downsample_pcd(input_pcd_path, output_pcd_path, voxel_size=0.02)